package com.onlyMe.world.path
{
	import com.qb9.flashlib.collections.PriorityQueue;
	import com.onlyMe.geom.Coord;
	
	import flash.utils.Dictionary;

	public class FourWayPathFinder implements PathFinder
	{
		protected var map:PathFinderMap;
		private var dest:Coord;
		
		private var cache:Dictionary;
		
		private var limit:int;
		
		public function FourWayPathFinder(limit:int=0)
		{
			this.limit = limit;
		}

		public function findPath(orig:Coord, dest:Coord, map:PathFinderMap):Array
		{
			this.map = map;
			this.dest = dest;
			
			cache = new Dictionary(true);

			var closed:Dictionary = new Dictionary(true);
			var attempts:int = 0;
			
			var q:PriorityQueue = new PriorityQueue();
			
			var closest:Array = [orig];
			var cost:Number = f(closest);
			q.enqueue(closest, 1/cost);
	
			while (!q.empty)
			{
				if (++attempts === limit)
					break;
					
				var p:Array = q.dequeue() as Array;
				var x:Coord = last(p);

				if (x in closed)
					continue;

				if (x.equals(dest))
					return p;

				closed[x] = true;

				closest = chooseClosest(closest, p);

				for each (var y:Coord in successors(x))
				{
					var np:Array = p.concat(y);
					cost = f(np);
					q.enqueue(np, 1/cost);
				}
			}

			return closest;
		}
		
		private function f(p:Array):Number
		{
			return totalCost(p) + map.getEstimatedCost(last(p), dest);
		}

		private function last(coords:Array):Coord
		{
			return coords[coords.length-1];
		}

		private function chooseClosest(p1:Array, p2:Array):Array
		{
			var dist1:Number = map.getEstimatedCost(last(p1), dest);
			var dist2:Number = map.getEstimatedCost(last(p2), dest);
			if (dist1 < dist2)
				return p1;
			if (dist1 > dist2)
				return p2;

			var cost1:Number = totalCost(p1);
			var cost2:Number = totalCost(p2);
			if (cost1 < cost2)
				return p1;
			if (cost1 > cost2)
				return p2;

			if (p1.length <= p2.length)
				return p1;
			return p2;
		}

		private function totalCost(path:Array):Number
		{
			var sum:Number = 0;
			var c1:Coord = path[0];
			for each(var c2:Coord in path.slice(1))
			{
				sum += map.getCost(c1, c2);
				c1 = c2;
			}
			return sum;
		}

		private function successors(c:Coord):Array
		{
			var ret:Array = [];
			for each (var d:Coord in getAdjacent(c))
				if (isFree(d))
					ret.push(d);
			return ret;
		}
		
		protected function isFree(coord:Coord):Boolean
		{
			if (coord in cache === false)
				cache[coord] = !map.isBlocked(coord);
			
			return cache[coord];
		}
		
		protected function getAdjacent(coord:Coord):Array
		{
			with (coord)
				return [
					Coord.create(x + 1,     y),
					Coord.create(x - 1,     y),
					Coord.create(    x, y + 1),
					Coord.create(    x, y - 1)
				];
		}
	}
}
